#include "pkt_proxy.h"

unsigned short htons(unsigned short number)
{
    unsigned short a = 0;
    
    a = ((number & 0x00FF) << 8) + ((number & 0xFF00) >> 8);
    
    return a;
}
    
unsigned int htonl(unsigned int number)
{
    unsigned int a = 0;
    
    a = ((number & 0x000000FF) << 24) + ((number & 0x0000FF00) << 8) + ((number & 0x00FF0000) >> 8) + ((number & 0xFF000000) >> 24);
    
    return a;
}

unsigned short ip_check_sum(unsigned short * pkt, unsigned short checksum_old)
{
    unsigned int sum = 0; //缓存sum
    
    int i;
    
    for(i = 0; i < 10; i++) //只计算20字节的IP头部
    {
        sum = sum + *pkt;
        pkt++;
    }
    
    sum = sum - checksum_old; //去掉之前的值
    
    sum = (sum & 0xFFFF) + (sum >> 16); //高16位加上低16位
    
    sum = ~sum; //取反
    
    return (unsigned short)sum;
}

unsigned short udp_check_sum(unsigned short * pkt, unsigned short checksum_old, unsigned short ip_pktlen, unsigned short udp_pktlen)
{
    unsigned int sum = 0; //缓存sum
    
    unsigned short count = 0; //循环次数
    
    count = udp_pktlen + 8; //多8个字节的循环数
    
    while(count > 1) //计算伪头部+数据
    {
        sum = sum + *pkt;
        pkt++;
        count = count - 2;
    }
    
    if(count > 0) sum = sum + *(unsigned char *)pkt; //奇数大小单独加字节
    
    sum = sum + 0x0011 + ip_pktlen - 0x0014 - checksum_old; //加上协议号(UDP: 17)，报文长度
    
    sum = (sum & 0xFFFF) + (sum >> 16); //高16位加上低16位
    
    sum = ~sum; //取反
    
    return (unsigned short)sum;
}

unsigned int arp_reply(unsigned char * request_pkt, unsigned char * reply_pkt)
{
    int i;
    unsigned char * p_compare = request_pkt;
    
    for(i = 0; i < 5; i++)
        if(*p_compare == 0xFF) p_compare++;
        else return 0;
    
    p_compare++;
    if((*(p_compare+8) == 0x00) && (*(p_compare+9) == 0x01))
        if((*(p_compare+14) == 0x00) && (*(p_compare+15) == 0x01))
        {
            for(i = 0; i < 6; i++)
                if(*(p_compare+i) == *(request_pkt+i+22)) continue;
                else return 0;
            
	    //* Node 2
            //if((*(p_compare+32) != 0xC0) || (*(p_compare+33) != 0xA8) || (*(p_compare+34) != 0x01) || (*(p_compare+35) != 0x02))
	    //* Node 3
            if((*(p_compare+32) != 0xC0) || (*(p_compare+33) != 0xA8) || (*(p_compare+34) != 0x01) || (*(p_compare+35) != 0x03))
                return 0;
                
            for(i = 0; i < 6; i++)
            {
                *reply_pkt = *p_compare;
                reply_pkt++;
                p_compare++;
            }
            
            p_compare = p_compare + 10;
            reply_pkt = reply_pkt + 26;
            
            for(i = 0; i < 10; i++)
            {
                *reply_pkt = *p_compare;
                reply_pkt++;
                p_compare++;
            }
        }
        else return 0;
    else return 0;
    
    return 42;
}

unsigned int udp_relay_tsmp(unsigned char * pkt, unsigned char * udp_pkt, unsigned int pkt_len)
{
    struct udp_pkt_hdr * p_udp_pkt_hdr = (struct udp_pkt_hdr *)pkt;
    struct udp_pkt_hdr * p_udp_send_pkt_hdr = (struct udp_pkt_hdr *)udp_pkt;
    
    int i;
    
    unsigned short len = 0;
    
    for(i = 0; i < 5; i++)
        if(p_udp_pkt_hdr->eth_hdr_part.dmac[i] == riscv_mac[i]) continue;
        else return 0;
    
    if(p_udp_pkt_hdr->ip_hdr_part.protocol != 17)
        return 0;
    
    if(p_udp_pkt_hdr->ip_hdr_part.dip != riscv_ip)
        return 0;
    
    if((p_udp_pkt_hdr->udp_hdr_part.dport != 0xB822) && \
      (p_udp_pkt_hdr->udp_hdr_part.dport != 0x611E) && \
      (p_udp_pkt_hdr->udp_hdr_part.dport != 0x0A1A))
        return 0;
    
    //缓存UDP信息
    for(i = 0; i < 6; i++)
        p_udp_send_pkt_hdr->eth_hdr_part.dmac[i] = p_udp_pkt_hdr->eth_hdr_part.smac[i];
    
    p_udp_send_pkt_hdr->ip_hdr_part.dip = p_udp_pkt_hdr->ip_hdr_part.sip;
    
    return (pkt_len-42);
}

unsigned int tsmp_relay_udp(unsigned char * pkt, unsigned char * udp_pkt, unsigned int pkt_len)
{
    struct udp_pkt_hdr * p_udp_send_pkt_hdr = (struct udp_pkt_hdr *)udp_pkt;
    unsigned char * p_payload = udp_pkt;
    
    unsigned short checksum_old;
    
    unsigned int i;
    
//    for(i = 0; i < 6; i++)
//        if(p_eth_hdr->dmac[i] == riscv_mac[i]) continue;
//        else return 0;
//    if(*(pkt + 14) != 0x02)
//        return 0;
 //     if((*(pkt + 14) == 0x04) && (*(pkt + 15) == 0x01))
  //      printf("get 0x0401!\r\n");
    
    if((*(pkt + 4) == 0x08) && (*(pkt + 5) == 0x01))
    {
        p_udp_send_pkt_hdr->udp_hdr_part.sport = 0xB822;
        p_udp_send_pkt_hdr->udp_hdr_part.dport = 0xB822;
    }
    else if((*(pkt + 4) == 0x08) && (*(pkt + 5) == 0x02))
    {
        p_udp_send_pkt_hdr->udp_hdr_part.sport = 0x611E;
        p_udp_send_pkt_hdr->udp_hdr_part.dport = 0x611E;
        printf("get 0x0802!\r\n");
    }
    //else if((*(pkt + 4) == 0x00) && (*(pkt + 5) == 0x03))
    //{
    //    p_udp_send_pkt_hdr->udp_hdr_part.sport = 0x0A1A;
    //    p_udp_send_pkt_hdr->udp_hdr_part.dport = 0x0A1A;
    //}
    else return 0;
    
    p_payload = p_payload + 42;
    for(i = 0; i < pkt_len; i++)
    {
        *p_payload = *pkt;
        p_payload++;
        pkt++;
    }
    
    p_udp_send_pkt_hdr->ip_hdr_part.tot_len = htons((unsigned short)(pkt_len + 8 + 20));
    p_udp_send_pkt_hdr->udp_hdr_part.length = htons((unsigned short)(pkt_len + 8));
    
    //计算IP校验和
    p_payload = udp_pkt;
    p_payload = p_payload + 14;
    checksum_old = p_udp_send_pkt_hdr->ip_hdr_part.checksum;
    p_udp_send_pkt_hdr->ip_hdr_part.checksum = ip_check_sum((unsigned short *)p_payload, checksum_old);
    
    //计算UDP校验和
    p_payload = udp_pkt;
    p_payload = p_payload + 26;
    checksum_old = p_udp_send_pkt_hdr->udp_hdr_part.checksum;
    p_udp_send_pkt_hdr->udp_hdr_part.checksum = 0x0000;//ip_check_sum((unsigned short *)p_payload, checksum_old);
    
    return (pkt_len+42); //udp_len - 8 + 14
}

unsigned short ip_csum(unsigned short* packet, int packlen) 
{
    register unsigned long sum = 0;
 
	//求和
	while (packlen > 1) {
    
		sum += *(packet++);
		packlen -= 2;
	}
 
	//因为两个字节合并unsigned short整型做计算，所以如果报文大小为奇数字节则单独加上最后一个字节的数据
	if (packlen > 0)
		sum += *(unsigned char*)packet;
 
	/* TODO: this depends on byte order */
 
	//如果存在溢出则截断高16位数据与低16位数据求和，如sum=0x0102030405 则sum=0x02030405 + 0x00000001
	while (sum >> 16)
		sum = (sum & 0xffff) + (sum >> 16);
 
	//sum取反码
	//printf("ip checksum:0x%04x\n", (unsigned short)~sum);
	return (unsigned short)~sum;
}
